import os
import sys
import shutil
import rospy
import threading
import json
import time
import math
import numpy as np
import scipy
from scipy import ndimage
import cv2
from typing import Any, List, Tuple, Iterable, Dict, Mapping, Callable, Union, Optional
from rich import print
from numba import jit


def get_node_address() -> str:
    return os.sep.join(sys.path[0].split(os.sep)[:-1])


def get_timestamp() -> float:
    return time.time()


def secs(ts: Union[np.ndarray, float, int]) -> Union[np.ndarray, float, int]:
    if isinstance(ts, np.ndarray):
        return np.floor(ts).astype(np.uint32)
    return int(math.floor(ts))


def nsecs(ts: Union[np.ndarray, float, int]) -> Union[np.ndarray, float, int]:
    if isinstance(ts, np.ndarray):
        return np.floor((ts - secs(ts)) * 1e9).astype(np.int64)
    return int(math.floor((ts - secs(ts)) * 1e9))


def sampling(ev: np.ndarray, sample_to: int = 1600) -> np.ndarray:
    sample_ratio = ev.shape[0] // sample_to + 1
    res = ev[::sample_ratio]
    return res


def resize_img(img: Optional[np.ndarray], size: Tuple[int, int]) -> np.ndarray:
    dummy = np.full(tuple([x for x in size] + [3]), 255, dtype = np.uint8)
    return cv2.resize(img, size, interpolation = cv2.INTER_NEAREST) if img is not None else dummy


class Variable:
    def __init__(self):
        self._var = dict()
    
    def get(self, key: str, def_val: Any) -> Any:
        if key not in self._var:
            return def_val
        return self._var[key]
    
    def set(self, key: str, val: Any) -> Any:
        self._var[key] = val
        return self._var[key]
    
    def delete(self, key: str) -> None:
        del self._var[key]


package_dir = get_node_address()
config_path = os.path.join(package_dir, "config", "config.json")
with open(config_path, "r") as f:
    config = json.load(f)